/***************************************************************************
 创建者: 华磊
 开始时间: 2019.11.23
 copyright: (C) 华友高科
 ***************************************************************************/
#include "joystickdelegate.h"
#include "joystickview.h"
#include <QDebug>
#include "powermanager.h"
#include "GeneralDefine.h"
#include "messagelog.h"
#include "domparser.h"

#define D_FULL_RANGE 32767
#define D_MIN_VALID_AXIS_VALUE 100

//#define D_JOYSTICK_DEVICE "/dev/input/js0"

extern PowerManager* p_powerManger;

JoyStickDelegate::JoyStickDelegate(QString configFileIn)
{

    isInJogMoving=false;

//    pressedKey=E_JOG_KEY_NONE;

    moveRatio_x=0;
    moveRatio_y=0;
    moveRatio_r=0;

    axis0Info.x=0;
    axis0Info.y=0;
    axis1Info.x=0;
    axis1Info.y=0;
    axis2Info.x=0;
    axis2Info.y=0;

    isLeftYOn=false;
    isRightYOn=false;

    manualSpeedRatio=0.2;

    joyStickView=new JoyStickView();

    connect(this, SIGNAL(setAxesValues_signal(QString ,QString ,QString ,QString )),
            joyStickView, SLOT(setAxesValues(QString ,QString ,QString ,QString)));
    connect(this, SIGNAL(setButtonState_signal(QString ,QString ,QString  )),
            joyStickView, SLOT(setButtonState(QString ,QString ,QString )));

    connect(this, SIGNAL(showView_signal(bool )),
            joyStickView, SLOT(showView_slot(bool )));

    if(1==initialConfigFile(configFileIn))
    {

        //初始化线程
        if(isUseJoyStick)
        {
//           displayJoyStickView_slot(true);
           start();
        }


    }
    else
    {
        addMsg( ENUM_MSG_ERROR,  "JoyStickDelegate", "JoyStickDelegate",  12610,  -1);
    }

}


void JoyStickDelegate::initConnect()
{
//    connect(joyStickView, SIGNAL(button_1P_pressedSignal()), this, SLOT(button_1P_pressedSlot()));
//    connect(joyStickView, SIGNAL(button_2P_pressedSignal()), this, SLOT(button_2P_pressedSlot()));
//    connect(joyStickView, SIGNAL(button_3P_pressedSignal()), this, SLOT(button_3P_pressedSlot()));
//    connect(joyStickView, SIGNAL(button_4P_pressedSignal()), this, SLOT(button_4P_pressedSlot()));
//    connect(joyStickView, SIGNAL(button_5P_pressedSignal()), this, SLOT(button_5P_pressedSlot()));
//    connect(joyStickView, SIGNAL(button_6P_pressedSignal()), this, SLOT(button_6P_pressedSlot()));

//    connect(joyStickView, SIGNAL(button_1N_pressedSignal()), this, SLOT(button_1N_pressedSlot()));
//    connect(joyStickView, SIGNAL(button_2N_pressedSignal()), this, SLOT(button_2N_pressedSlot()));
//    connect(joyStickView, SIGNAL(button_3N_pressedSignal()), this, SLOT(button_3N_pressedSlot()));
//    connect(joyStickView, SIGNAL(button_4N_pressedSignal()), this, SLOT(button_4N_pressedSlot()));
//    connect(joyStickView, SIGNAL(button_5N_pressedSignal()), this, SLOT(button_5N_pressedSlot()));
//    connect(joyStickView, SIGNAL(button_6N_pressedSignal()), this, SLOT(button_6N_pressedSlot()));

//    connect(joyStickView, SIGNAL(button_releasedSignal()), this, SLOT(button_releasedSlot()));

//    connect(joyStickView, SIGNAL(clearMsgAndServoOn_signal()), this, SLOT(clearMsgAndServoOnSlot()));
//    connect(joyStickView, SIGNAL(speedUp_signal()), this, SLOT(setManualSpeedPlusSlot()));
//    connect(joyStickView, SIGNAL(speedDown_signal()), this, SLOT(setManualSpeedReduceSlot()));
//    connect(joyStickView, SIGNAL(startOrStop_signal()), this, SLOT(runSlot()));
////    connect(joyStickView, SIGNAL(newOrder_signal()), this, SLOT(newOrder_signalSlot()));
//    connect(joyStickView, SIGNAL(setJointCoordinate_signal()), this, SLOT(setJointCoordinateSlot()));
//    connect(joyStickView, SIGNAL(modeChange_signal()), this, SLOT(setAllRunModel()));

//    connect(this,SIGNAL(cease_signal()),SLOT(programAllCease()));

//    connect(joyStickView, SIGNAL(lInstruction()), this, SIGNAL(lInstruction()));


}

void JoyStickDelegate::displayJoyStickView_slot(bool isShow)
{

    if(isShow)
    {
        joyStickView->show();

    }
    else
    {
        joyStickView->hide();
    }
}

int JoyStickDelegate::playJoystickMusic(QString musicNameIn, int musicPlayTimes, int musicVolume)
{
    #ifdef D_USE_ROS
    QString musicFileName = QString("%1%2").arg(JOYSTICK_MUSIC_FILEPATH).
            arg(musicNameIn);//.remove(QRegExp("\\s")

    // 终止之前音乐进程
    system("killall -9 madplay &");

    if(musicPlayTimes == 0)
    {
        // 暂停播放
        system("killall -9 madplay &");
//        isMusicPlaying = false;

    }
    else if(musicPlayTimes == -1)
    {
        // 循环播放
        QString currentRuningMusicCommand=(QString("%1%2%3%4")
                                   .arg("madplay -a ")
                                   .arg(QString::number(conversionVolume(musicVolume)))
                                   .arg(" " + musicFileName)
                                   .arg(" -r &"));
        system(currentRuningMusicCommand.toLatin1().data());
//        isMusicPlaying = true;
    }
    else
    {
        // 播放一次
        system((QString("%1%2%3%4")
                .arg("madplay -a ")
                .arg((QString::number(conversionVolume(musicVolume))))
                .arg(" " + musicFileName)
                .arg(" &")).toLatin1().data());
//        isMusicPlaying = true;
    }
    std::cout << "The Music File :" << musicFileName.toStdString() << std::endl;
    #endif
    return 1;

}

void JoyStickDelegate::stopMusic()
{
    #ifdef D_USE_ROS
//    if(isMusicPlaying)
//    {
        // 终止音乐
        system("killall -9 madplay &");
//        isMusicPlaying = false;
//        currentRuningMusicCommand.clear();
//    }
#endif
}

int JoyStickDelegate::conversionVolume(int volumeIn)
{
    // 音量转换(-175 ~ +18)取( -60 ~ +0)
//    return (int)((70.0/100.0)*((double)volumeIn)) - 52;//最大１８可能导致无法播放出声音。
     return 60*((double)volumeIn)/100.0-60;
}


int JoyStickDelegate::initialConfigFile(QString configFileName)
{
    qDebug() << "JoyStickDelegate::initialConfigFile ==== 初始化配置文件：" << configFileName;

    DomParser domparser;
    bool ok = domparser.openXml(configFileName, QIODevice::ReadOnly);
    if( !ok )
    {
        return -1;
    }

    QDomNode domNode = domparser.findSubNode(domparser.getRootDomElement(), "isUseJoyStick", ok );
    if( !ok )
    {
            return -1;
    }
    domparser.readXml(domNode, isUseJoyStick);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "isForAgvControl", ok );
    if( !ok )
    {

            return -1;
    }
    domparser.readXml(domNode, isForAgvControl);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "isPlayNoticeSound", ok );
    if( !ok )
    {

            return -1;
    }
    domparser.readXml(domNode, isPlayNoticeSound);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "joystickDevName", ok );
    if( !ok )
    {

            return -1;
    }
    domparser.readXml(domNode, joystickDevName);


    domparser.closeXml();
    return 1;
}


void JoyStickDelegate::joyStickMonitor_agv()
{

    //判断手动按钮
    if(false==isInJogMoving)
    {
        if(isJoyStickJogNeedStart())
        {
            isInJogMoving=true;

            qDebug()<<"JoyStickDelegate::vehicleJog";
            addMsg( ENUM_MSG_REMIND, "JoyStickDelegate", "JoyStickDelegate", 12605, -1);
            if(isPlayNoticeSound)
            {
                playJoystickMusic("jogRun.mp3",-1,100);
            }

        }
    }
    else
    {


        if(false==isJoyStickJogNeedStop())
        {

            updateJogValueFromAxis(moveRatio_x,moveRatio_y,moveRatio_r);
            qDebug()<<"JoyStickDelegate::vehicleJog x"<<moveRatio_x
                   <<"moveRatio_y"<<moveRatio_y<<"moveRatio_r"<<moveRatio_r<<"manualSpeedRatio"<<manualSpeedRatio;
             p_powerManger->vehicleJog(userInfo,1,moveRatio_x*manualSpeedRatio,
                                       moveRatio_y*manualSpeedRatio,moveRatio_r*manualSpeedRatio);
//            communicateNormal->jogRunDogSlot();//手动按下时，一直喂狗
        }
        else
        {

            stopJogMove();//没有按下时，立马停止

        }

    }



}


void JoyStickDelegate::stopJogMove()
{
    qDebug()<<"JoyStickDelegate::stopJogMove ";
    moveRatio_x=0;
    moveRatio_y=0;
    moveRatio_r=0;

    if(isInJogMoving)
    {
        qDebug()<<"JoyStickDelegate::stopJogMove";
        p_powerManger->jogStop(userInfo);
//        communicateSaveThread->stopWatchDog();
//        emit stopRunSignal();//会调用停止程序，导致手动轴时迅速停止。
//        communicateSaveThread->pauseRunSlot();
        if(isPlayNoticeSound)
        {
            stopMusic();
        }
    }
    isInJogMoving=false;

}

bool JoyStickDelegate::isJoyStickJogNeedStop()
{
    if(isInJogMoving)
    {
        if(fabs(axis0Info.x)<D_MIN_VALID_AXIS_VALUE && fabs(axis0Info.y)<D_MIN_VALID_AXIS_VALUE
               && false==isLeftYOn && false==isRightYOn
                 && fabs(axis2Info.x)<D_MIN_VALID_AXIS_VALUE && fabs(axis2Info.y)<D_MIN_VALID_AXIS_VALUE)
        {
            return true;
        }
        else
        {
            return false;
        }
    }
    else
    {
        return false;
    }

}

bool JoyStickDelegate::isJoyStickJogNeedStart()
{
    if(isInJogMoving)
    {
        return false;
    }
    else
    {
        if(fabs(axis0Info.x)>D_MIN_VALID_AXIS_VALUE || fabs(axis0Info.y)>D_MIN_VALID_AXIS_VALUE
               || isLeftYOn || isRightYOn
                 || fabs(axis2Info.x)>D_MIN_VALID_AXIS_VALUE || fabs(axis2Info.y)>D_MIN_VALID_AXIS_VALUE)
        {
            return true;
        }
        else
        {
            return false;
        }

    }
}

void JoyStickDelegate::updateJogValueFromAxis(double &xOut,double &yOut,double &rOut)
{
//    JoyStickAxisInfo axis2Info;//X 和　Ｒ ,固定速度
//    JoyStickAxisInfo axis0Info;//X 和　Ｒ，可调速度
//    JoyStickAxisInfo axis1Info;//X 和　Y，可调速度

    //xOut
    if(fabs(axis2Info.y)>fabs(axis0Info.y))
    {
        xOut=axis2Info.y/D_FULL_RANGE;
    }
    else
    {
        xOut=axis0Info.y/D_FULL_RANGE;
    }


    //yOut
    if(true==isLeftYOn && false==isRightYOn)
    {
        yOut=-1;
    }
    else if(false==isLeftYOn && true==isRightYOn)
    {
        yOut=1;
    }
    else
    {
        yOut=0;
    }


    //rOut
    if(fabs(axis2Info.x)>fabs(axis0Info.x))
    {
        rOut=axis2Info.x/D_FULL_RANGE;

    }
    else
    {
        rOut=axis0Info.x/D_FULL_RANGE;
    }

    xOut=-xOut;
    rOut=-rOut;
}

void JoyStickDelegate::addMsg(int messageLevel, string componentName, string messageType, int messageCode, int robotId, int parameter1, int parameter2, int parameter3, int parameter4, QString strIn)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotId;
    tmpMsg.messageCode = messageCode;



    QString infomationStr;

    switch(messageCode)
    {
    case 12612:
    {
        infomationStr =  QObject::tr("手柄模块：操作手柄已拔出！");
        break;
    }
    case 12611:
    {
        infomationStr =  QObject::tr("手柄模块：操作手柄已插入！");
        break;
    }
    case 12610:
    {
        infomationStr =  QObject::tr("手柄模块：joyStickConfig.xml文件读取失败！");
        break;
    }
    case 12609:
    {
        infomationStr =  QObject::tr("手柄模块：关闭障碍物检测！");
        break;
    }
    case 12608:
    {
        infomationStr =  QObject::tr("手柄模块：开启障碍物检测！");
        break;
    }
    case 12607:
    {
        infomationStr =  QObject::tr("手柄模块：手动速度修改！");
        break;
    }
    case 12606:
    {
        infomationStr =  QObject::tr("手柄模块：复位错误并使能电机！");
        break;
    }
    case 12605:
    {
        infomationStr =  QObject::tr("手柄模块：手动移动agv！");
        break;
    }
    case 12604:
    {
        infomationStr =  QObject::tr("手柄模块：启动程序！");
        break;
    }
    case 12603:
    {
        infomationStr =  QObject::tr("手柄模块：停止程序！");
        break;
    }
    case 12602:
    {
        infomationStr =  QObject::tr("手柄模块：暂停程序！");
        break;
    }
    case 12601:
    {
        infomationStr =  QObject::tr("手柄模块：切换为调度模式！");
        break;
    }
    case 12600:
    {
        infomationStr =  QObject::tr("手柄模块：切换为手动T2模式！");
        break;
    }
    default:

        break;
    }


    tmpMsg.MessageInformation = infomationStr.toStdString();

    MessageLog::getInstance()->addMessage(tmpMsg);
}

void JoyStickDelegate::setManualSpeedPlusSlot()
{
    if(manualSpeedRatio<=0.08)
    {
        manualSpeedRatio+=0.015;
    }
    else if(manualSpeedRatio<=3)
    {
        manualSpeedRatio+=0.08;
    }
    else if(manualSpeedRatio<=6)
    {
        manualSpeedRatio+=0.1;
    }
    else
    {
        manualSpeedRatio+=0.12;
    }

    if(manualSpeedRatio>1)
    {
        manualSpeedRatio=1;
    }
//    p_powerManger->setManualSpeedPercent(userInfo,);
//    communicateNormal->setManualSpeedSlot(globalData->globalManSpeed+deltaSpeed);
    qDebug()<<"--------------------------manualSpeedRatio"<<manualSpeedRatio;
    if(isPlayNoticeSound)
    {
        playJoystickMusic("jogRatioIncrease.mp3",1,100);
    }
}

void JoyStickDelegate::setManualSpeedReduceSlot()
{

    if(manualSpeedRatio<=0.08)
    {
        manualSpeedRatio-=0.015;
    }
    else if(manualSpeedRatio<=3)
    {
        manualSpeedRatio-=0.08;
    }
    else if(manualSpeedRatio<=6)
    {
        manualSpeedRatio-=0.1;
    }
    else
    {
        manualSpeedRatio-=0.12;
    }

    if(manualSpeedRatio<=0)
    {
        manualSpeedRatio=0.01;
    }

//    p_powerManger->setManualSpeedPercent(userInfo,);
//    communicateNormal->setManualSpeedSlot(globalData->globalManSpeed-deltaSpeed);
    qDebug()<<"-----------------------manualSpeedRatio"<<manualSpeedRatio;
    if(isPlayNoticeSound)
    {
        playJoystickMusic("jogRatioReduce.mp3",1,100);
    }
}


void JoyStickDelegate::runSlot()
{


}

int JoyStickDelegate::programAllCease()//暂停
{

    qDebug()<<"JoyStickDelegate::programAllCease";
    addMsg( ENUM_MSG_REMIND, "JoyStickDelegate", "JoyStickDelegate", 12602, -1);
    p_powerManger->programCease_fleet();
}

void JoyStickDelegate::setJointCoordinateSlot()
{
   //globalData->globalCoordinate
   //EM_JOG_COORDINATE_JOINT=0,
   //EM_JOG_COORDINATE_TOOL=1,
   //EM_JOG_COORDINATE_USER=2,
   //EM_JOG_COORDINATE_WORLD=3
   //communicateNormal->setJointCoordinateSlot(jointCoordinate);
//    int jointCoordinate;
//    if(globalData->globalCoordinate == EM_JOG_COORDINATE_JOINT)
//    {
//        communicateNormal->setJointCoordinateSlot(EM_JOG_COORDINATE_TOOL);
//    }
//    else if(globalData->globalCoordinate == EM_JOG_COORDINATE_TOOL)
//    {
//        communicateNormal->setJointCoordinateSlot(EM_JOG_COORDINATE_USER);
//    }
//    else if(globalData->globalCoordinate == EM_JOG_COORDINATE_USER)
//    {
//        communicateNormal->setJointCoordinateSlot(EM_JOG_COORDINATE_WORLD);
//    }
//    else if(globalData->globalCoordinate == EM_JOG_COORDINATE_WORLD)
//    {
//        communicateNormal->setJointCoordinateSlot(EM_JOG_COORDINATE_JOINT);
//    }

}

int JoyStickDelegate::setAllRunModel()
{
//    p_powerManger
//    globalData->globalHandState
//    EM_CONTROL_MODE_T1=0,
//    EM_CONTROL_MODE_T2=1,
//    EM_CONTROL_MODE_AUTO=2,
//    EM_CONTROL_MODE_EXT=3,
//    EM_CONTROL_MODE_FLEET=4
//    communicateNormal->setAllRunModel(runModel,returnFlag);
//    int returnFlag;
//    if(globalData->globalHandState == EM_CONTROL_MODE_T1){
//        communicateNormal->setAllRunModel(EM_CONTROL_MODE_T2,returnFlag);
//    }
//    else if(globalData->globalHandState == EM_CONTROL_MODE_T2){
//        communicateNormal->setAllRunModel(EM_CONTROL_MODE_AUTO,returnFlag);
//    }
//    else if(globalData->globalHandState == EM_CONTROL_MODE_AUTO){
//        communicateNormal->setAllRunModel(EM_CONTROL_MODE_EXT,returnFlag);
//    }
//    else if(globalData->globalHandState == EM_CONTROL_MODE_EXT){
//        communicateNormal->setAllRunModel(EM_CONTROL_MODE_FLEET,returnFlag);
//    }
//    else if(globalData->globalHandState == EM_CONTROL_MODE_FLEET){
//        communicateNormal->setAllRunModel(EM_CONTROL_MODE_T1,returnFlag);
//    }
}





static  void __check_head(struct list_head* head)
{
    if ((head->next == 0) && (head->prev == 0)) {
        INIT_LIST_HEAD(head);
    }
}

/*
 * Insert a new entry between two known consecutive entries.
 *
 * This is only for internal list manipulation where we know
 * the prev/next entries already!
 */
static  void __list_add(struct list_head* pre,
                        struct list_head* prev,
                        struct list_head* next)
{

    next->prev = pre;
    pre->next = next;
    pre->prev = prev;
    prev->next = pre;

}


/*
 * Delete a list entry by making the prev/next entries
 * point to each other.
 *
 * This is only for internal list manipulation where we know
 * the prev/next entries already!
 */
static  void __list_del(struct list_head* prev,
                        struct list_head* next)
{

    next->prev = prev;
    prev->next = next;
}



/**
 * list_add - add a new entry
 * @new: new entry to be added
 * @head: list head to add it after
 *
 * Insert a new entry after the specified head.
 * This is good for implementing stacks.
 */
void list_add(struct list_head* pre, struct list_head* head)
{
    __check_head(head);
    __list_add(pre, head, head->next);
}

/**
 * list_add_tail - add a new entry
 * @new: new entry to be added
 * @head: list head to add it before
 *
 * Insert a new entry before the specified head.
 * This is useful for implementing queues.
 */
void list_add_tail(struct list_head* pre, struct list_head* head)
{
    __check_head(head);
    __list_add(pre, head->prev, head);
}


/**
 * list_del - deletes entry from list.
 * @entry: the element to delete from the list.
 * Note: list_empty on entry does not return true after this, the entry is in an undefined state.
 */
void list_del(struct list_head* entry)
{
    __list_del(entry->prev, entry->next);
}

/**
 * list_del_init - deletes entry from list and reinitialize it.
 * @entry: the element to delete from the list.
 */
void list_del_init(struct list_head* entry)
{
    __list_del(entry->prev, entry->next);
    INIT_LIST_HEAD(entry);
}

/**
 * list_move - delete from one list and add as another's head
 * @list: the entry to move
 * @head: the head that will precede our entry
 */
void list_move(struct list_head* list, struct list_head* head)
{
    __check_head(head);
    __list_del(list->prev, list->next);
    list_add(list, head);
}

/**
 * list_move_tail - delete from one list and add as another's tail
 * @list: the entry to move
 * @head: the head that will follow our entry
 */
void list_move_tail(struct list_head* list,
                    struct list_head* head)
{
    __check_head(head);
    __list_del(list->prev, list->next);
    list_add_tail(list, head);
}


/**
 * list_splice - join two lists
 * @list: the new list to add.
 * @head: the place to add it in the first list.
 */
void list_splice(struct list_head* list, struct list_head* head)
{
    struct list_head* first = list;
    struct list_head* last  = list->prev;
    struct list_head* at    = head->next;

    first->prev = head;
    head->next  = first;

    last->next = at;
    at->prev   = last;
}

struct list_head* list_dequeue( struct list_head* list ) {
    struct list_head* next, *prev, *result = ((list_head*)0);

    prev = list;
    next = prev->next;

    if ( next != prev ) {
        result = next;
        next = next->next;
        next->prev = prev;
        prev->next = next;
        result->prev = result->next = result;
    }

    return result;
}


#if 1
#define LOG_DBG(fmt, ...)  fprintf(stdout, fmt, ##__VA_ARGS__)
#else
#define LOG_DBG(fmt, ...)
#endif

#define LOG_ERR(fmt, ...)  fprintf(stderr, fmt, ##__VA_ARGS__)


typedef struct _joy_stick_ctx {
struct list_head list;
int i4_js_fd;
unsigned int i4_op_block;
} JOYSTICK_CTX_T;

LIST_HEAD(_t_js_ctx_head);
/*==>  struct list_head _t_js_ctx_head = {&_t_js_ctx_head, &_t_js_ctx_head};*/

int joystick_open(char* cp_js_dev_name, int i4_block)
{
int i4_open_flags = O_RDONLY;
JOYSTICK_CTX_T*  pt_joystick_ctx = NULL;

if (!cp_js_dev_name) {
    LOG_ERR("[%s] js device name is NULL\n", __func__);
    return -1;
}

pt_joystick_ctx = (JOYSTICK_CTX_T*)calloc(sizeof(JOYSTICK_CTX_T), 1);
if (!pt_joystick_ctx) {
    LOG_ERR("[%s] no memory!!\n", __func__);
    return -1;
}

pt_joystick_ctx->i4_op_block = i4_block ? 1 : 0;

if (pt_joystick_ctx->i4_op_block == 0) {
    i4_open_flags |= O_NONBLOCK;
}

pt_joystick_ctx->i4_js_fd = open(cp_js_dev_name, i4_open_flags);
if (pt_joystick_ctx->i4_js_fd < 0) {
    LOG_ERR("[%s] open device %s error\n", __func__, cp_js_dev_name);
    free(pt_joystick_ctx);
    return -1;
}

list_add_tail(&pt_joystick_ctx->list, &_t_js_ctx_head);

return pt_joystick_ctx->i4_js_fd;
}

int joystick_close(int i4_fd)
{
struct list_head* pt_entry;
struct list_head* pt_next;
JOYSTICK_CTX_T* pt_node;

if (list_empty(&_t_js_ctx_head)) {
//    LOG_ERR("[%s] device not opened\n", __func__);
    return -1;
}

list_for_each_safe(pt_entry, pt_next, &_t_js_ctx_head) {
    pt_node = list_entry(pt_entry, JOYSTICK_CTX_T, list);
    if (pt_node->i4_js_fd == i4_fd) {
        list_del_init(&pt_node->list);
        free(pt_node);
        return close(i4_fd);
    }
}

LOG_ERR("[%s] i4_fd=%d invalid\n", __func__, i4_fd);
return -1;
}

int joystick_read_one_event(int i4_fd, struct js_event* tp_jse)
{
int i4_rd_bytes;

/*do not check i4_fd again*/

i4_rd_bytes = read(i4_fd, tp_jse, sizeof(struct js_event));

if (i4_rd_bytes == -1) {
    if (errno == EAGAIN)
    {
        /*when no block, it is not error*/
        return 0;
    }
    else {
        return -1;
    }
}

return i4_rd_bytes;
}

int joystick_read_ready(int i4_fd)
{
int i4_block = 2;
struct list_head* pt_entry;
JOYSTICK_CTX_T* pt_node;

if (list_empty(&_t_js_ctx_head)) {
//    LOG_ERR("[%s] device not opened\n", __func__);
    return -1;
}

list_for_each(pt_entry, &_t_js_ctx_head) {
    pt_node = list_entry(pt_entry, JOYSTICK_CTX_T, list);
    if (pt_node->i4_js_fd == i4_fd) {
        i4_block = pt_node->i4_op_block;
        break;
    }
}

if (i4_block == 2) {
    LOG_ERR("[%s] i4_fd=%d invalid\n", __func__, i4_fd);
    return 0;
}
else if (i4_block == 1) {
    fd_set readfd;
    int i4_ret = 0;
    struct timeval timeout = {0, 0};
    FD_ZERO(&readfd);
    FD_SET(i4_fd, &readfd);

    i4_ret = select(i4_fd + 1, &readfd, NULL, NULL, &timeout);

    if (i4_ret > 0 && FD_ISSET(i4_fd, &readfd)) {
        return 1;
    }
    else {
        return 0;
    }

}

return 1; /*noblock read, aways ready*/
}


void debug_list(void)
{
if (! list_empty(&_t_js_ctx_head)) {
    struct list_head* pt_entry;
    JOYSTICK_CTX_T* pt_node;

    list_for_each(pt_entry, &_t_js_ctx_head) {
        pt_node = list_entry(pt_entry, JOYSTICK_CTX_T, list);
        LOG_DBG("fd:%d--block:%d\n", pt_node->i4_js_fd, pt_node->i4_op_block);
    }
}
else {
    LOG_DBG("-----------> EMPTY NOW\n");
}
}


#include <unistd.h>
#include <sys/syscall.h>
#define gettid() syscall(__NR_gettid)

void JoyStickDelegate::run()
{
    qDebug()<<"thread JoyStickDelegate::run";
//    setPriority(QThread::NormalPriority);
    qDebug()<<"************************JoyStickDelegate pid="<<gettid();
    sleep(3);
    if(false==isUseJoyStick)
    {
        return  ;
    }
    bool isJoyStickInitialOk=false;
    bool isJoyStickInitialOk_old=false;
    if(false==isJoyStickInitialOk)
    {
        if(initialJoyStickDevice())
        {
            isJoyStickInitialOk_old=isJoyStickInitialOk;
            isJoyStickInitialOk=true;
        }
    }

    QFile file(joystickDevName);
    int tmpCount=0;
    while(1)
    {
        QThread::msleep(150);
        updateJoyStickKeyValue();
        if(isForAgvControl)
        {
            joyStickMonitor_agv();
        }

        //设备插入判断
        if(tmpCount<30)
        {
            tmpCount++;
        }
        else
        {
            tmpCount=0;
            if( false==isJoyStickInitialOk)
            {
                if(true==file.exists() )
                {
                    isJoyStickInitialOk_old=isJoyStickInitialOk;
                    if(initialJoyStickDevice())
                    {

                        isJoyStickInitialOk=true;
                    }
                    else
                    {
                        isJoyStickInitialOk=false;
                    }

                    if(false==isJoyStickInitialOk_old && true==isJoyStickInitialOk)
                    {
                        qDebug()<<"JoyStickDelegate:: joystick plugIn";
                        addMsg( ENUM_MSG_REMIND, "JoyStickDelegate", "JoyStickDelegate", 12611, -1);
                        if(isPlayNoticeSound)
                        {
                            playJoystickMusic("joystickPlugIn.mp3",1,100);
                        }
                        emit showView_signal(true);
                    }
                }


            }
            else if( true==file.exists() && true==isJoyStickInitialOk)
            {

            }
            else if( false==file.exists() && false==isJoyStickInitialOk)
            {

            }
            else if( false==file.exists() && true==isJoyStickInitialOk)
            {
                isJoyStickInitialOk_old=isJoyStickInitialOk;
                isJoyStickInitialOk=false;
                if(true==isJoyStickInitialOk_old && false==isJoyStickInitialOk)
                {
                    qDebug()<<"JoyStickDelegate:: joystick plugOut";
                    addMsg( ENUM_MSG_REMIND, "JoyStickDelegate", "JoyStickDelegate", 12612, -1);
                    if(isPlayNoticeSound)
                    {
                        playJoystickMusic("joystickPlugOut.mp3",1,100);
                    }
                }
                emit showView_signal(false);
            }
            else
            {



            }
        }





    }

}



void JoyStickDelegate::updateJoyStickKeyValue()
{


    do
    {
        if (joystick_read_ready(fd))
        {
            rc = joystick_read_one_event(fd, &jse);
            if (rc > 0)
            {
                if ((jse.type & JS_EVENT_INIT) == JS_EVENT_INIT) {
                    if ((jse.type & ~JS_EVENT_INIT) == JS_EVENT_BUTTON) {
                        if (jse.value) {
                            buttons_state |= (1 << jse.number);
                        }
                        else {
                            buttons_state &= ~(1 << jse.number);
                        }
                    }
                    else if ((jse.type & ~JS_EVENT_INIT) == JS_EVENT_AXIS) {
                        if (tp_axes) {
                            if ((jse.number & 1) == 0) {
                                tp_axes[jse.number / 2].x = jse.value;
                            }
                            else {
                                tp_axes[jse.number / 2].y = jse.value;
                            }
                        }

                    }
                }
                else
                {
                    op_times++;
                    if (print_init_stat == 0) {
                        for (i = 0; i < number_of_btns; i++) {
    //                        LOG_DBG("joystick init state: button %d is %s.\n", i, ((buttons_state & (1 << i)) == (1 << i)) ? "DOWN" : "UP");
                        }

                        if (tp_axes)
                            for (i = 0; i < number_of_axes; i++) {
    //                            LOG_DBG("joystick init state: axes %d is x=%d  y=%d.\n", i, tp_axes[i].x, tp_axes[i].y);
                            }
                        print_init_stat = 1;
                    }

                    if (jse.type  == JS_EVENT_BUTTON)
                    {
                        if (jse.value) {
                            buttons_state |= (1 << jse.number);
                        }
                        else {
                            buttons_state &= ~(1 << jse.number);
                        }

                        //LOG_DBG("joystick state: button %d is %s.\n", jse.number, ((buttons_state & (1 << jse.number)) == (1 << jse.number)) ? "DOWN" : "UP");
    //                    qDebug() << QString("joystick state: button %1 is %2").arg(jse.number).arg(((buttons_state & (1 << jse.number)) == (1 << jse.number)) ? "DOWN" : "UP");

                        QString contentStr = QString("joystick state: button %1 is %2").arg(jse.number).arg(((buttons_state & (1 << jse.number)) == (1 << jse.number)) ? "DOWN" : "UP");
                        QString btnIndex = QString("%1").arg(jse.number);
                        QString btnState = QString("%1").arg(((buttons_state & (1 << jse.number)) == (1 << jse.number)) ? "DOWN" : "UP");

                        if(isForAgvControl)
                        {
                            setButtonState_agv(contentStr,btnIndex,btnState);
                        }

                        emit setButtonState_signal(contentStr,btnIndex,btnState);
                    }
                    else if (jse.type == JS_EVENT_AXIS)
                    {
                        if (tp_axes) {
                            if ((jse.number & 1) == 0) {
                                tp_axes[jse.number / 2].x = jse.value;
                            }
                            else {
                                tp_axes[jse.number / 2].y = jse.value;
                            }
                            //LOG_DBG("joystick state: axes %d is x=%d  y=%d.\n", jse.number / 2, tp_axes[jse.number / 2].x, tp_axes[jse.number / 2].y);
    //                        qDebug()<< QString("joystick state: axes %1 is x=%2  y=%3").arg(jse.number / 2).arg(tp_axes[jse.number / 2].x).arg(tp_axes[jse.number / 2].y);

                            QString content = QString("joystick state: axes %1 is x=%2  y=%3").arg(jse.number / 2).arg(tp_axes[jse.number / 2].x).arg(tp_axes[jse.number / 2].y);
                            QString axesAaaa = QString("%1").arg(jse.number / 2);
                            QString axesXvalue = QString("%2").arg(tp_axes[jse.number / 2].x);
                            QString axesYvalue = QString("%3").arg(tp_axes[jse.number / 2].y);

                            setAxesValues(content,axesAaaa,axesXvalue,axesYvalue);
                            emit setAxesValues_signal(content,axesAaaa,axesXvalue,axesYvalue);
                        }
                        else
                        {
                            //LOG_DBG("joystick state: axes %d is %s=%d.\n", jse.number / 2, ((jse.number & 1) == 0) ? "x" : "y", jse.value);
    //                    qDebug()<< QString("joystick state: axes %1 is %s=%2").arg(jse.number / 2).arg(((jse.number & 1) == 0) ? "x" : "y", jse.value);

                        }
                    }
                }

            }
            else
            {
                break;
            }

        }
        else
        {
            break;
        }
        usleep(1000);
    }
    while(1);


}

void JoyStickDelegate::setButtonState_agv(QString contStr, QString index, QString state)
{
    qDebug()<<"JoyStickView button index"<<index<<state;

    if(index == "0")
    {
        if(state == "DOWN")
        {

            qDebug()<<"JoyStickDelegate::clearMsgAndServoOnSlot";
            addMsg( ENUM_MSG_REMIND, "JoyStickDelegate", "JoyStickDelegate", 12606, -1);
            p_powerManger->setAllMsgClear_fleet();
            p_powerManger->servoOnCurrentRobot();
            if(isPlayNoticeSound)
            {
                playJoystickMusic("clearMessageAndServoOn.mp3",1,100);
            }

        }
        else
        {

        }
    }

    else if(index == "1")
    {
        if(state == "DOWN")
        {

            qDebug()<<"JoyStickDelegate::setAllRunModel";
            addMsg( ENUM_MSG_REMIND, "JoyStickDelegate", "JoyStickDelegate", 12601, -1);
            int returnFlag;
            p_powerManger->programReset_fleet();
            p_powerManger->setAllRunModel(userInfo,ENUM_CONTROL_FLEET,returnFlag);
            if(isPlayNoticeSound)
            {
                playJoystickMusic("changeToFleetMode.mp3",1,100);
            }

        }
        else
        {

        }
    }

    else if(index == "2")
    {
        if(state == "DOWN")
        {
            int returnFlag;
            p_powerManger->setUltrasonicSensorCheckEnable(userInfo,false,returnFlag);
            p_powerManger->setPointCloudCheckEnable(userInfo,false,returnFlag);
            addMsg( ENUM_MSG_REMIND, "JoyStickDelegate", "JoyStickDelegate", 12609, -1);

            if(isPlayNoticeSound)
            {
                playJoystickMusic("barrierCheckDisable.mp3",1,100);
            }
        }
        else
        {

        }
    }

    else if(index == "3")
    {
        if(state == "DOWN")
        {

            //
            int returnFlag;
            p_powerManger->setUltrasonicSensorCheckEnable(userInfo,true,returnFlag);
            p_powerManger->setPointCloudCheckEnable(userInfo,true,returnFlag);
            addMsg( ENUM_MSG_REMIND, "JoyStickDelegate", "JoyStickDelegate", 12608, -1);

            if(isPlayNoticeSound)
            {
                playJoystickMusic("barrierCheckEnable.mp3",1,100);
            }
        }
        else
        {

        }
    }

    else if(index == "4")
    {
        if(state == "DOWN")
        {
            setManualSpeedReduceSlot();
        }
        else
        {

        }
    }

    else if(index == "5")
    {
        if(state == "DOWN")
        {
            setManualSpeedPlusSlot();
        }
        else
        {

        }
    }

    else if(index == "6")
    {
        if(state == "DOWN")
        {
            isLeftYOn=true;
        }
        else
        {
            isLeftYOn=false;
        }
    }

    else if(index == "7")
    {
        if(state == "DOWN")
        {
            isRightYOn=true;


        }
        else
        {
            isRightYOn=false;
        }
    }

    else if(index == "8")
    {
        if(state == "DOWN")
        {
            qDebug()<<"JoyStickDelegate::setAllRunModel";
            addMsg( ENUM_MSG_REMIND, "JoyStickDelegate", "JoyStickDelegate", 12600, -1);
            p_powerManger->programReset_fleet();
            int returnFlag;
            p_powerManger->setAllRunModel(userInfo,ENUM_CONTROL_T2,returnFlag);

            if(isPlayNoticeSound)
            {
                playJoystickMusic("changeToT2Mode.mp3",1,100);
            }
        }
        else
        {

        }
    }

    else if(index == "9")
    {
        if(state == "DOWN")
        {
            qDebug()<<"JoyStickDelegate::setAllRunModel";
            addMsg( ENUM_MSG_REMIND, "JoyStickDelegate", "JoyStickDelegate", 12604, -1);
            p_powerManger->setAllMsgClear_fleet();
            p_powerManger->servoOnCurrentRobot();
            p_powerManger->programRun_fleet();
            if(isPlayNoticeSound)
            {
                playJoystickMusic("programRun.mp3",1,100);
            }
        }
        else
        {

        }
    }

    else if(index == "10")
    {
        if(state == "DOWN")
        {
            stopJogMove();
        }
        else
        {

        }
    }

    else if(index == "11")
    {

    }
    else if(index == "12")
    {

    }
}

void JoyStickDelegate::setAxesValues(QString contStr, QString index_axes, QString coordinates_x,
                                     QString coordinates_y)
{
    qDebug()<<"JoyStickDelegate::setAxesValues index_axes"<<index_axes<<coordinates_x<<coordinates_y;
    if(index_axes == "2")
    {
        axis2Info.x=coordinates_x.toInt();
        axis2Info.y=coordinates_y.toInt();
        if(coordinates_x == "-32767" && coordinates_y == "0")
        {

        }
        else if(coordinates_x == "32767" && coordinates_y == "0")
        {

        }
        else if(coordinates_x == "0" && coordinates_y == "-32767")
        {

        }
        else if(coordinates_x == "0" && coordinates_y == "32767")
        {

        }
        else
        {

        }
    }


    if(index_axes == "0")
    {
        axis0Info.x=coordinates_x.toInt();
        axis0Info.y=coordinates_y.toInt();
        if(coordinates_x == "0" && coordinates_y == "-32767")
        {

        }
        else if(coordinates_x == "0" && coordinates_y == "32767")
        {

        }
        else
        {

        }
    }


    if(index_axes == "1")
    {
        axis1Info.x=coordinates_x.toInt();
        axis1Info.y=coordinates_y.toInt();
        if(coordinates_x == "0" && coordinates_y == "-32767")
        {

        }
        else if(coordinates_x == "0" && coordinates_y == "32767")
        {

        }
        else
        {

        }
    }
}

int JoyStickDelegate::initialJoyStickDevice()
{

    op_times = 0;
    number_of_axes = 0;
    number_of_btns = 0;
    js_name_str[128];
    buttons_state = 0;
    tp_axes = NULL;
    i=0;
    print_init_stat = 0;

    //struct js_event jse;

    fd = joystick_open(joystickDevName.toLatin1().data(), 1);
    if (fd < 0)
    {
        LOG_ERR("open failed.\n");
        return 0;
    }

    rc = ioctl(fd, JSIOCGAXES, &number_of_axes);
    if (rc != -1) {
        LOG_DBG("number_of_axes:%d\n", number_of_axes);
        if (number_of_axes > 0) {
            tp_axes = (AXES_T*)calloc(sizeof(AXES_T), 1);
        }
    }

    rc = ioctl(fd, JSIOCGBUTTONS, &number_of_btns);
    if (rc != -1) {
        qDebug () << QString("number_of_btns:%1\n").arg(number_of_btns);
    }

    if (ioctl(fd, JSIOCGNAME(sizeof(js_name_str)), js_name_str) < 0) {
        LOG_DBG(js_name_str, "Unknown", sizeof(js_name_str));
    }
    qDebug () << QString("joystick Name: %1\n").arg(js_name_str);


    return 1;
}
